/*
 * Copyright (C) 2014, Field and Service Robotics 2014, Group 6.
 *
 *  Servo Ctrl
 */

#include "servoCtrl.h"
	
#define MANIP_RAISED 0
#define MANIP_LOWERED 87
#define MANIP_HOVERED 75
#define GRIPPER_CLOSED 115
#define GRIPPER_OPENED 0

void setParams()
{
    ROS_INFO("Setting parameters to server");
    ros::NodeHandle nh;

    nh.setParam("servo_ctrl_angle_manipulator_raised",      180.0);
    nh.setParam("servo_ctrl_angle_manipulator_lowered",     10.0);
    nh.setParam("servo_ctrl_angle_gripper_opened",          95.0);
    nh.setParam("servo_ctrl_angle_gripper_closed",          50.0);
    nh.setParam("servo_ctrl_angle_ptu_pan",                 90.0);
    nh.setParam("servo_ctrl_angle_ptu_tilt",                45.0);
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "servoCtrl");

    ros::NodeHandle nodeHandle;
    setParams();

    ServoCtrl sc(nodeHandle);
    sc.setHealth(ServoCtrl::NOT_READY);

    ros::Rate loop_rate(10);

    while(ros::ok())
    {
        ros::spinOnce();
        loop_rate.sleep();
    }

    return 0;
}

ServoCtrl::ServoCtrl (ros::NodeHandle& nh)
{
    mNodeHandle = nh;

    // initialize healthcheck service
    health_check_service = nh.advertiseService("servoctrl_health_check", &ServoCtrl::healthCheck, this);
    ROS_INFO("Service health_check succesfully initiated");

    // initialize publishers
    ptu_servo_pub = nh.advertise<geometry_msgs::Vector3>("ptu_servo_angles", 1);
    ROS_INFO("Publisher ptu_servo succesfully initiated");
    manip_servo_pub = nh.advertise<geometry_msgs::Vector3>("manip_servo_angles", 1);
    ROS_INFO("Publisher manip_servo succesfully initiated");

    // initialize services to listen for service calls
    // test them with rostopic call ...
    servo_camera_service = nh.advertiseService("transform_camera_servo", &ServoCtrl::transformCamera, this);
    ROS_INFO("Service transform_camera_servo succesfully initiated");
    servo_manipulator_service = nh.advertiseService("transform_manipulator_servo", &ServoCtrl::transformManipulator, this);
    ROS_INFO("Service transform_manipulator_servo succesfully initiated");

    health = INIT_SUCCESSFULL;
}

bool ServoCtrl::runServo(int servo) {
    
    geometry_msgs::Vector3 rotation;

    if (servo == SERVO_PTU) {

        rotation.z = cameraTiltAngle;
        rotation.y = cameraPanAngle;
        rotation.x = 0.0;

        ptu_servo_pub.publish(rotation);

        ROS_INFO("ptu service called and published");

        return true;

    } else if (servo == SERVO_MANIP) {

        rotation.x = gripperAngle;
        rotation.y = liftAngle;
        rotation.z = 0.0;

        manip_servo_pub.publish(rotation);

        ROS_INFO("manipulator service called and published");

        return true;

    } else {
        ROS_ERROR("This servo doesnt exist!");
        return false;
    }

    return false;
}

bool ServoCtrl::transformCamera(group6::transformCameraServo::Request &req, group6::transformCameraServo::Response &res)
{
    bool success = false;
    bool correctAngleGiven = true;

    // set the correct angle based on the action given
    if (req.action == PAN_CAMERA) {
        cameraPanAngle = req.rotation;
    } else if (req.action == TILT_CAMERA) {
        cameraTiltAngle = req.rotation;
    } else {
        ROS_ERROR("This action doesnt exist!");
        correctAngleGiven = false;      
    }

    if (correctAngleGiven) {
        success = runServo(SERVO_PTU);

        if (!success) {
            health = SERVO_ERROR;
        }
    } else {
        health = ACTION_ERROR;
    }

    res.success = success;

    return success;
}

bool ServoCtrl::transformManipulator(group6::transformManipulatorServo::Request &req, group6::transformManipulatorServo::Response &res)
{
    bool success = false;
    bool correctAngleGiven = true;
	bool paramServerSuccess = false;

    // set the correct angle based on the action given
    if (req.action == RAISE_MANIP) {
        liftAngle = MANIP_RAISED;//mNodeHandle.getParam("servo_ctrl_angle_manipulator_raised", liftAngle);
    } else if (req.action == LOWER_MANIP) {
        liftAngle = MANIP_LOWERED;//mNodeHandle.getParam("servo_ctrl_angle_manipulator_lowered", liftAngle);
    } else if (req.action == GRAB) {
	      gripperAngle = GRIPPER_CLOSED;
    } else if (req.action == RELEASE) {
        gripperAngle = GRIPPER_OPENED; //mNodeHandle.getParam("servo_ctrl_angle_gripper_opened", gripperAngle);
    } else if (req.action == HOVER_MANIP) {
	liftAngle = MANIP_HOVERED;
    } else {
        ROS_ERROR("This action doesnt exist!");
        correctAngleGiven = false;      
    }

    if (correctAngleGiven) {
        success = runServo(SERVO_MANIP);

        if (!success) {
            health = SERVO_ERROR;
        } else {
            health = SERVO_SUCCESS;
        }
    } else {
        health = ACTION_ERROR;
    }

    //res.state = health;
    res.state = SERVO_SUCCESS;

    return success;
}

bool ServoCtrl::healthCheck(group6::healthCheck::Request &req, group6::healthCheck::Response &res) {
    
    res.health = health;
    return true;   
}

void ServoCtrl::setHealth(int h) {
    health = h;
}
